# pragma once

# include "cpp_tools.h"
# include "dataStruct.h"

std::tuple<Eigen::VectorXd,Eigen::VectorXd,Eigen::VectorXd> 
vehicle_body_dynamic(Eigen::VectorXd X, Eigen::VectorXd U, data::vehicle_param sys)
{
    int i = 0;
    double v = X(i++);
    double beta = X(i++);
    double omega = X(i++);

    i = 0;
    double delta_f_1 = U(i++);
    double delta_f_2 = U(i++);
    double delta_r_1 = U(i++);
    double delta_r_2 = U(i++);

    double F_x_f_1 = U(i++);
    double F_x_f_2 = U(i++);
    double F_x_r_1 = U(i++);
    double F_x_r_2 = U(i++);

    double F_y_f_1 = U(i++);
    double F_y_f_2 = U(i++);
    double F_y_r_1 = U(i++);
    double F_y_r_2 = U(i++);

    double mass = sys.m_body.mass_sprung + sys.m_body.mass_unsprung_front + sys.m_body.mass_unsprung_rear
        + 4*sys.m_tire.mass_wheel;
    double inertial_z = sys.m_body.inertial_z;
    double l_f = sys.m_body.l_f;
    double l_r = sys.m_body.l_r;
    double l_y = sys.m_body.l_y;
    double h_cg = sys.m_body.h_cg;
    double g = sys.m_common.g;
    double l_x = l_f + l_r;

    double D_v = 1/mass*(
        F_x_f_1*std::cos(delta_f_1 - beta) + F_x_f_2*std::cos(delta_f_2 - beta)
        + F_x_r_1*std::cos(delta_r_1 - beta) + F_x_r_2*std::cos(delta_r_2 - beta)
        - F_y_f_1*std::sin(delta_f_1 - beta) - F_y_f_2*std::sin(delta_f_2 - beta)
        - F_y_r_1*std::sin(delta_r_1 - beta) - F_y_r_2*std::sin(delta_r_2 - beta)
    );

    double D_beta = 0;
    if (std::abs(v) >= 0.01)
    {
        D_beta = 1/(mass*v)*(
            F_x_f_1*std::sin(delta_f_1 - beta) + F_x_f_2*std::sin(delta_f_2 - beta)
            + F_x_r_1*std::sin(delta_r_1 - beta) + F_x_r_2*std::sin(delta_r_2 - beta)
            + F_y_f_1*std::cos(delta_f_1 - beta) + F_y_f_2*std::cos(delta_f_2 - beta)
            + F_y_r_1*std::cos(delta_r_1 - beta) + F_y_r_2*std::cos(delta_r_2 - beta)
        ) - omega;
    }
    
    double D_omega = 1/inertial_z*(
        l_y/2*(-F_x_f_1*std::cos(delta_f_1) + F_y_f_1*std::sin(delta_f_1))
        + l_y/2*(F_x_f_2*std::cos(delta_f_2) - F_y_f_2*std::sin(delta_f_2))
        + l_y/2*(-F_x_r_1*std::cos(delta_r_1) + F_y_r_1*std::sin(delta_r_1))
        + l_y/2*(F_x_r_2*std::cos(delta_r_2) - F_y_r_2*std::sin(delta_r_2))
        + l_f*(F_x_f_1*std::sin(delta_f_1) + F_y_f_1*std::cos(delta_f_1))
        + l_f*(F_x_f_2*std::sin(delta_f_2) + F_y_f_2*std::cos(delta_f_2))
        - l_r*(F_x_r_1*std::sin(delta_r_1) + F_y_r_1*std::cos(delta_r_1))
        - l_r*(F_x_r_2*std::sin(delta_r_2) + F_y_r_2*std::cos(delta_r_2))
    );

    double a_x = D_v*std::cos(beta) - (D_beta + omega)*v*std::sin(beta);
    double a_y = (D_beta + omega)*v*std::cos(beta) + D_v*std::sin(beta);

    double F_z_f_1_static = mass*(l_r*g/(2*l_x) - h_cg*a_x/(2*l_x) - h_cg*a_y/(2*l_y));
    double F_z_f_2_static = mass*(l_r*g/(2*l_x) - h_cg*a_x/(2*l_x) + h_cg*a_y/(2*l_y));
    double F_z_r_1_static = mass*(l_r*g/(2*l_x) + h_cg*a_x/(2*l_x) - h_cg*a_y/(2*l_y));
    double F_z_r_2_static = mass*(l_r*g/(2*l_x) + h_cg*a_x/(2*l_x) + h_cg*a_y/(2*l_y));

    Eigen::VectorXd D_X(3);
    D_X << D_v,D_beta,D_omega;

    Eigen::VectorXd F_z_0(4);
    F_z_0 << F_z_f_1_static,F_z_f_2_static,F_z_r_1_static,F_z_r_2_static;

    Eigen::VectorXd acc(2);
    acc << a_x,a_y;

    return {D_X,F_z_0,acc};
}